// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2009 Thomas Capricelli <orzel@freehackers.org>
// Copyright (C) 2012 Desire Nuentsa <desire.nuentsa_wakam@inria.fr>
//
// This code initially comes from MINPACK whose original authors are:
// Copyright Jorge More - Argonne National Laboratory
// Copyright Burt Garbow - Argonne National Laboratory
// Copyright Ken Hillstrom - Argonne National Laboratory
//
// This Source Code Form is subject to the terms of the Minpack license
// (a BSD-like license) described in the campaigned CopyrightMINPACK.txt file.

#ifndef EIGEN_LMQRSOLV_H
#define EIGEN_LMQRSOLV_H

namespace Eigen {

namespace internal {

template<typename Scalar, int Rows, int Cols, typename PermIndex>
void
lmqrsolv(Matrix<Scalar, Rows, Cols>& s,
		 const PermutationMatrix<Dynamic, Dynamic, PermIndex>& iPerm,
		 const Matrix<Scalar, Dynamic, 1>& diag,
		 const Matrix<Scalar, Dynamic, 1>& qtb,
		 Matrix<Scalar, Dynamic, 1>& x,
		 Matrix<Scalar, Dynamic, 1>& sdiag)
{
	/* Local variables */
	Index i, j, k;
	Scalar temp;
	Index n = s.cols();
	Matrix<Scalar, Dynamic, 1> wa(n);
	JacobiRotation<Scalar> givens;

	/* Function Body */
	// the following will only change the lower triangular part of s, including
	// the diagonal, though the diagonal is restored afterward

	/*     copy r and (q transpose)*b to preserve input and initialize s. */
	/*     in particular, save the diagonal elements of r in x. */
	x = s.diagonal();
	wa = qtb;

	s.topLeftCorner(n, n).template triangularView<StrictlyLower>() = s.topLeftCorner(n, n).transpose();
	/*     eliminate the diagonal matrix d using a givens rotation. */
	for (j = 0; j < n; ++j) {

		/*        prepare the row of d to be eliminated, locating the */
		/*        diagonal element using p from the qr factorization. */
		const PermIndex l = iPerm.indices()(j);
		if (diag[l] == 0.)
			break;
		sdiag.tail(n - j).setZero();
		sdiag[j] = diag[l];

		/*        the transformations to eliminate the row of d */
		/*        modify only a single element of (q transpose)*b */
		/*        beyond the first n, which is initially zero. */
		Scalar qtbpj = 0.;
		for (k = j; k < n; ++k) {
			/*           determine a givens rotation which eliminates the */
			/*           appropriate element in the current row of d. */
			givens.makeGivens(-s(k, k), sdiag[k]);

			/*           compute the modified diagonal element of r and */
			/*           the modified element of ((q transpose)*b,0). */
			s(k, k) = givens.c() * s(k, k) + givens.s() * sdiag[k];
			temp = givens.c() * wa[k] + givens.s() * qtbpj;
			qtbpj = -givens.s() * wa[k] + givens.c() * qtbpj;
			wa[k] = temp;

			/*           accumulate the transformation in the row of s. */
			for (i = k + 1; i < n; ++i) {
				temp = givens.c() * s(i, k) + givens.s() * sdiag[i];
				sdiag[i] = -givens.s() * s(i, k) + givens.c() * sdiag[i];
				s(i, k) = temp;
			}
		}
	}

	/*     solve the triangular system for z. if the system is */
	/*     singular, then obtain a least squares solution. */
	Index nsing;
	for (nsing = 0; nsing < n && sdiag[nsing] != 0; nsing++) {
	}

	wa.tail(n - nsing).setZero();
	s.topLeftCorner(nsing, nsing).transpose().template triangularView<Upper>().solveInPlace(wa.head(nsing));

	// restore
	sdiag = s.diagonal();
	s.diagonal() = x;

	/* permute the components of z back to components of x. */
	x = iPerm * wa;
}

template<typename Scalar, int _Options, typename Index>
void
lmqrsolv(SparseMatrix<Scalar, _Options, Index>& s,
		 const PermutationMatrix<Dynamic, Dynamic>& iPerm,
		 const Matrix<Scalar, Dynamic, 1>& diag,
		 const Matrix<Scalar, Dynamic, 1>& qtb,
		 Matrix<Scalar, Dynamic, 1>& x,
		 Matrix<Scalar, Dynamic, 1>& sdiag)
{
	/* Local variables */
	typedef SparseMatrix<Scalar, RowMajor, Index> FactorType;
	Index i, j, k, l;
	Scalar temp;
	Index n = s.cols();
	Matrix<Scalar, Dynamic, 1> wa(n);
	JacobiRotation<Scalar> givens;

	/* Function Body */
	// the following will only change the lower triangular part of s, including
	// the diagonal, though the diagonal is restored afterward

	/*     copy r and (q transpose)*b to preserve input and initialize R. */
	wa = qtb;
	FactorType R(s);
	// Eliminate the diagonal matrix d using a givens rotation
	for (j = 0; j < n; ++j) {
		// Prepare the row of d to be eliminated, locating the
		// diagonal element using p from the qr factorization
		l = iPerm.indices()(j);
		if (diag(l) == Scalar(0))
			break;
		sdiag.tail(n - j).setZero();
		sdiag[j] = diag[l];
		// the transformations to eliminate the row of d
		// modify only a single element of (q transpose)*b
		// beyond the first n, which is initially zero.

		Scalar qtbpj = 0;
		// Browse the nonzero elements of row j of the upper triangular s
		for (k = j; k < n; ++k) {
			typename FactorType::InnerIterator itk(R, k);
			for (; itk; ++itk) {
				if (itk.index() < k)
					continue;
				else
					break;
			}
			// At this point, we have the diagonal element R(k,k)
			//  Determine a givens rotation which eliminates
			//  the appropriate element in the current row of d
			givens.makeGivens(-itk.value(), sdiag(k));

			// Compute the modified diagonal element of r and
			// the modified element of ((q transpose)*b,0).
			itk.valueRef() = givens.c() * itk.value() + givens.s() * sdiag(k);
			temp = givens.c() * wa(k) + givens.s() * qtbpj;
			qtbpj = -givens.s() * wa(k) + givens.c() * qtbpj;
			wa(k) = temp;

			// Accumulate the transformation in the remaining k row/column of R
			for (++itk; itk; ++itk) {
				i = itk.index();
				temp = givens.c() * itk.value() + givens.s() * sdiag(i);
				sdiag(i) = -givens.s() * itk.value() + givens.c() * sdiag(i);
				itk.valueRef() = temp;
			}
		}
	}

	// Solve the triangular system for z. If the system is
	// singular, then obtain a least squares solution
	Index nsing;
	for (nsing = 0; nsing < n && sdiag(nsing) != 0; nsing++) {
	}

	wa.tail(n - nsing).setZero();
	//     x = wa;
	wa.head(nsing) = R.topLeftCorner(nsing, nsing).template triangularView<Upper>().solve /*InPlace*/ (wa.head(nsing));

	sdiag = R.diagonal();
	// Permute the components of z back to components of x
	x = iPerm * wa;
}
} // end namespace internal

} // end namespace Eigen

#endif // EIGEN_LMQRSOLV_H
